#include "astar.h"
#include <vector>

#define abs(x) ((x) < 0 ? -(x) : (x))

enum directions{UP, DOWN, RIGHT, LEFT};

list<Point> FindRoute(Point src, Point dst, char map[MAP_SIZE][MAP_SIZE]){
    list<Point> route;
    // int deep; // 避免搜索过久，暂时没用

    bool visited[MAP_SIZE][MAP_SIZE] = {false}; // 点是否访问过
    priority_queue<TreeNode*, vector<TreeNode*>, TreeNodeCompare> heap; // 优先级队列，队首为走的代价最少的点
    vector<TreeNode*> freelist; // 存储树以便于释放内存
    bool isFind = false; // 查找成功标志

    TreeNode* nodeCurrent = new TreeNode(src, 0); //记录当前点
    TreeNode* nodeNew = nullptr; //试探节点,用于试探下一个位置的点
    freelist.push_back(nodeCurrent);

    while(1){
        // 4个方向
        for(int i = 0;i < 4;i++){
            Point pNew = Point(nodeCurrent->p);
            switch(i){
                case UP: pNew.y--;break;
                case DOWN: pNew.y++;break;
                case RIGHT: pNew.x++;break;
                case LEFT: pNew.x--;break;
            }
            if(isValidPoint(map, visited, pNew)){
                nodeNew = new TreeNode(pNew, nodeCurrent->d + 1);
                // 计算代价
                nodeNew->cost = (abs(nodeNew->p.x - dst.x) + abs(nodeNew->p.y - dst.y)) + nodeNew->d;
                nodeNew->parent = nodeCurrent;

                heap.push(nodeNew);
                freelist.push_back(nodeNew);
                visited[pNew.x][pNew.y] = true;
            }
        }
        if(heap.size() == 0){
            break; // 没有找到终点
        }
        nodeCurrent = heap.top();  // 取堆中代价最小的点继续往下走
        heap.pop();
        if(nodeCurrent->p == dst){
            isFind = true;
            break;
        }
    }

    if(isFind){
        while(nodeCurrent->parent != nullptr){
            route.push_front(nodeCurrent->p);
            nodeCurrent = nodeCurrent->parent;
        }
    }

    // 释放内存
    for (const auto& node : freelist) {
        delete node;
    }
    return route;
}

bool isValidPoint(char map[MAP_SIZE][MAP_SIZE], bool visited[MAP_SIZE][MAP_SIZE], const Point& p){
    // 墙或者水
    if (map[p.x][p.y] == '*' || map[p.x][p.y] == '#') return false;
    // 已经走过
    if (visited[p.x][p.y]) return false;
    // 越界
    if (p.x < 0 || p.y < 0 || p.x >= MAP_SIZE || p.y >= MAP_SIZE) return false;
    return true;//否则能走
}